#include "CombineJoint.h"
#include <omp.h>


CombineJoint::CombineJoint()
{

}




void CombineJoint::MergeJoint(const StepID &i,const MJointData &j)
{
    std::lock_guard<std::mutex> lock(keyMutexMap.getMutexForKey(i));
    int chainID = j.chainID;
    auto &RJT = m_d[i];

    MJointData::Merge(RJT,j);

//    bool ret = MJointData::checkAny(RJT,chainID);
//    if(!ret){

//    }else{
//        // std::cerr << "已经有Joint数据了" << std::endl;
//    }

}

void CombineJoint::MergeCXJ(const StepID &i, const CXJ &j)
{
    for(auto &_J:j){
        MergeJoint(i,_J);
    }
}

void CombineJoint::Start(std::string csv)
{
    ReadCSV(csv);
    std::cout << "读取CSV结束，开始整合."<< std::endl;
    // omp_set_num_threads(8);
    // omp_set_nested(1);

    std::cout << "Start Merge:";
    // #pragma omp parallel for schedule(dynamic, 4) num_threads(4)
    for(auto & j:csvJJ)
    {
        MergeOne(j);
        std::cout <<  '.';
    }

    std::cout <<  "End."<< std::endl;

    // serialJt(m_d);
}

void CombineJoint::CreateEmptyJoint(int st)
{

}

void  CombineJoint::ReadCSV(std::string csv)
{
    auto ret = readCSV(csv);
    sch.OpenDB("runDB/mids.db");
    std::cout << __FUNCTION__;
    for(auto &r:ret){
        CSVRowJoint j;
        j.station = r.station == "SA" ? 1:2;
        j.startStep = r.startTime;
        j.endStep = r.endTime;
        j.typ = stringToTaskType(r.robotType);
        j.duraing = r.duration;
        j.wrlID = r.wrlID;
        j.partID = r.partID;
        j.ReadJointByJson(r.axisData);
        csvJJ.emplace_back(j);
        std::cout <<  '.';
    }
    std::cout <<  "End." << std::endl;
    keyMutexMap.CreatMax(csvJJ.back().endStep);
}

void CombineJoint::MergeOne(const CSVRowJoint &cj)
{
    // #pragma omp parallel for schedule(dynamic, 4) num_threads(4)
    for(int i = 0;i< cj.duraing;i++){
        StepID real =  i +cj.startStep;
        MergeJoint(real,cj.joints.at(i));
    }
}

void CombineJoint::SerialALL()
{
    nlohmann::json j = nlohmann::json::array();
    for ( const auto& jd : m_d)
        j.push_back(serializeMJointDataV(jd.second));

    std::string filePath = "Plan/runData/ALL.json";
    std::ofstream file(filePath);

    if (!file.is_open()) {
        throw std::runtime_error("无法打开文件: " + filePath);
    }

    file << j.dump(4); // 使用缩进使输出更易读（4个空格）
    file.close();
    std::cout << "恭喜,执行完成了." << std::endl;
}

void CSVRowJoint::ReadJointByJson(JointJsonPack js)
{
    joints = deserializeMJointDataV(js);
    if(joints.empty()) return;
    int chainOri = joints.front().chainID;
    real_chain = sch.GetChainByStation(station,typ,chainOri);

    if(typ == TASK_PART_PICE){
        real_chain = sch.GetChainByID(wrlID,partID);
    }
    duraing = joints.size();

    if(real_chain<0){
        std::cerr << "Err" <<std::endl;
    }

    for(auto &jx:joints){
        jx.UpdateChainID(real_chain);
        if(typ == TASK_PART_PICE){
            // jx.moved = true;

            jx.ClearJoint();
        }
    }
}
